// 引入必要的头文件
#include "../board/sdk_project_config.h"
#include "../board/clock_config.h"
#include "../board/motor_control.h"
#include "../board/pin_config.h"
#include "../board/global_vars.h"
#include "../board/uart_interface.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#define MOTOR_DUTY 0x4000  // 约25%占空比
#define ADJUST_TIME_MS 400 // 单次调整时间
#define FORWARD_TIME_MS 800 // 单次前进时间

// 运动控制函数（开环PWM）
void car_forward(int ms) {
    StopAllMotors();
    SetMotor1Direction(FORWARD); SetMotor1Speed(MOTOR_DUTY);
    SetMotor2Direction(FORWARD); SetMotor2Speed(MOTOR_DUTY);
    SetMotor3Direction(FORWARD); SetMotor3Speed(MOTOR_DUTY);
    SetMotor4Direction(FORWARD); SetMotor4Speed(MOTOR_DUTY);
    DELAY_MS(ms);
    StopAllMotors();
}

void car_backward(int ms) {
    StopAllMotors();
    SetMotor1Direction(BACKWARD); SetMotor1Speed(MOTOR_DUTY);
    SetMotor2Direction(BACKWARD); SetMotor2Speed(MOTOR_DUTY);
    SetMotor3Direction(BACKWARD); SetMotor3Speed(MOTOR_DUTY);
    SetMotor4Direction(BACKWARD); SetMotor4Speed(MOTOR_DUTY);
    DELAY_MS(ms);
    StopAllMotors();
}

void car_turn_left(int ms) {
    StopAllMotors();
    SetMotor1Direction(BACKWARD); SetMotor1Speed(MOTOR_DUTY);
    SetMotor2Direction(BACKWARD); SetMotor2Speed(MOTOR_DUTY);
    SetMotor3Direction(FORWARD);  SetMotor3Speed(MOTOR_DUTY);
    SetMotor4Direction(FORWARD);  SetMotor4Speed(MOTOR_DUTY);
    DELAY_MS(ms);
    StopAllMotors();
}

void car_turn_right(int ms) {
    StopAllMotors();
    SetMotor1Direction(FORWARD);  SetMotor1Speed(MOTOR_DUTY);
    SetMotor2Direction(FORWARD);  SetMotor2Speed(MOTOR_DUTY);
    SetMotor3Direction(BACKWARD); SetMotor3Speed(MOTOR_DUTY);
    SetMotor4Direction(BACKWARD); SetMotor4Speed(MOTOR_DUTY);
    DELAY_MS(ms);
    StopAllMotors();
}

// 供uart_interface.c调用的姿态调整回调
void Vision_AdjustByK230(int pos, float angle) {
    static int last_pos = 0;
    printf("解析结果: 位置=%d, 角度=%.2f\r\n", pos, angle);
    if (angle > 10) {
        printf("右偏，右转校正\r\n");
        car_turn_right(ADJUST_TIME_MS);
    } else if (angle < -10) {
        printf("左偏，左转校正\r\n");
        car_turn_left(ADJUST_TIME_MS);
    } else if (pos != last_pos && pos != 0) {
        printf("位置变化，前进一小步\r\n");
        car_forward(FORWARD_TIME_MS);
        last_pos = pos;
    } else {
        printf("姿态正常，保持静止\r\n");
        StopAllMotors();
    }
}

int main(void) {
    CLOCK_SYS_Init(&g_stClockManInitConfig);
    PINS_DRV_Init(NUM_OF_CONFIGURED_PINS, g_stPinmuxConfigArr);
    UART_Interface_Init();
    DELAY_MS(500);
    printf("开环视觉姿态调整系统启动，等待K230二维码定位数据...\r\n");

    // 依次测试四个轮子，正转和反转各1秒
    printf("测试电机1正转\r\n");
    StopAllMotors();
    SetMotor1Direction(FORWARD); SetMotor1Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);
    printf("测试电机1反转\r\n");
    SetMotor1Direction(BACKWARD); SetMotor1Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);

    printf("测试电机2正转\r\n");
    SetMotor2Direction(FORWARD); SetMotor2Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);
    printf("测试电机2反转\r\n");
    SetMotor2Direction(BACKWARD); SetMotor2Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);

    printf("测试电机3正转\r\n");
    SetMotor3Direction(FORWARD); SetMotor3Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);
    printf("测试电机3反转\r\n");
    SetMotor3Direction(BACKWARD); SetMotor3Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);

    printf("测试电机4正转\r\n");
    SetMotor4Direction(FORWARD); SetMotor4Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);
    printf("测试电机4反转\r\n");
    SetMotor4Direction(BACKWARD); SetMotor4Speed(MOTOR_DUTY);
    DELAY_MS(1000);
    StopAllMotors(); DELAY_MS(500);

    printf("单轮测试结束，进入主循环\r\n");
    while (1) {
        UART_ReceiveLoop();
        DELAY_MS(10);
    }
}
